#ifndef LQR_VMC_CONTROLLER__LQR_VMC_HPP_
#define LQR_VMC_CONTROLLER__LQR_VMC_HPP_

#include <Eigen/Dense>
#include <cmath>
#include <vector>

namespace lqr_vmc_controller
{
class lqr_vmc
{
private:
    // 控制器更新频率
    double update_rate_;  
    // 机器人反馈位移
    double robot_displacement_;
    // 机器人反馈Pitch (此LQR算法中,后倾为负，前倾为正)
    double robot_pitch_;
    // 机器人反馈速度
    double robot_velocity_;
    // 机器人反馈Pitch角速度
    double robot_pitchome_;
    // 机器人反馈Yaw (左转为正，右转为负)
    double robot_yaw_;
    // 机器人反馈Yaw角速度
    double robot_yawome_;
    // 大腿关节角度偏置
    double robot_thigh_position_offset_;
    // 机器人上身拟合质点质量
    double robot_m_;
    // 轮距
    double wheel_separation_;

    // 腿部连杆参数
    double L_11_;
    double L_12_;
    double L_2_;
    double L_3_;
    double L_4_;
    // right_vmc腿长误差积分
    double L_error_integral_right_;
    // left_vmc腿长误差积分
    double L_error_integral_left_;
    // right_vmcPID过去腿长误差
    double L_error_previous_right_;
    // left_vmcPID过去腿长误差
    double L_error_previous_left_;
    // right_vmc L_error_dot滤波上次采样值
    double L_error_dot_right_previous_;
    // left_vmc L_error_dot滤波上次采样值
    double L_error_dot_left_previous_;

    // rollPD过去roll角度误差
    double roll_error_previous_;
    // rollPD角度误差积分
    double roll_error_intergral_;
    // roll滤波上次采样值
    double roll_error_dot_previous_;

public:
    lqr_vmc();
    // 设置反馈值
    void setlqrfeedback(double robot_displacement, double robot_pitch, double robot_velocity, double robot_pitchome, double robot_yaw, double robot_yawome);
    // lqr算法乘以增益矩阵得到车轮扭矩
    Eigen::Matrix<double, 2 ,1> lqr(double right_thigh_theta, double left_thigh_theta);
    // vmc算法求解大腿扭矩(结合roll轴前馈加PD)
    double vmc_right(double thigh_angle_d, double thigh_angle, double roll_PD_L, double roll_d, double roll);
    double vmc_left(double thigh_angle_d, double thigh_angle, double roll_PD_L, double roll_d, double roll);
    // roll轴PD
    double roll_PD(double roll_angle_d, double roll_angle);
    // 质心初始pitch偏角
    double initial_pitch(double right_thigh_angle, double left_thigh_angle);

};

}

# endif

